using System;
using System.Collections.Generic;
using System.Text;
using surfInterop;
using System.Drawing;
using System.Xml.Serialization;
using DotNetMatrix;

namespace Blimpbots.Recognition
{
    [Serializable]
    public class NamedPoint
    {
        public Point Point;
        public double Distance;
        public NamedPoint(Point Point, double Distance)
        { this.Point = Point; this.Distance = Distance; }
        public NamedPoint() { }
    }

    public struct CorrespondingKeypoint
    {
        public int Example;
        public int Idx;
        public CorrespondingKeypoint(int Example, int Idx)
        {
            this.Example = Example;
            this.Idx = Idx;
        }
    }

    public abstract class RecognitionExample
    {
        [XmlIgnore]
        public Dictionary<string, NamedPoint> NamedPoints 
            = new Dictionary<string, NamedPoint>();

        [XmlIgnore]
        protected List<CorrespondingKeypoint>[] _corresp = null;

        [XmlIgnore]
        public List<CorrespondingKeypoint>[] Correspondences
        {
            get { return _corresp; }
            set { _corresp = value; }
        }

        public List<Point> ObjectOutline = new List<Point>();
        public int[] KeypointStrength;
        public double[] MaxMatchError;

        protected double _width, _height;
        public double Width { get { return _width; } }
        public double Height { get { return _height; } }

        [XmlIgnore]
        private Nullable<RectangleF> _boundingBox = null;

        public RectangleF BoundingBox
        {
            get
            {
                if (_boundingBox != null) return _boundingBox.Value;

                double min_x = double.MaxValue, min_y = double.MaxValue,
                               max_x = 0, max_y = 0;

                // If no outline is available, estimate one
                // with the keypoint data
                if (ObjectOutline == null || ObjectOutline.Count == 0)
                {
                    foreach (SurfKeypoint k in _modelKeypoints)
                    {
                        min_x = Math.Min(k.x, min_x);
                        min_y = Math.Min(k.y, min_y);
                        max_x = Math.Max(k.x, max_x);
                        max_y = Math.Max(k.y, max_y);
                    }
                }
                else // use the information from the user-specified outline
                {
                    foreach (Point pt in ObjectOutline)
                    {
                        min_x = Math.Min((double)pt.X, min_x);
                        min_y = Math.Min((double)pt.Y, min_y);
                        max_x = Math.Max((double)pt.X, max_x);
                        max_y = Math.Max((double)pt.Y, max_y);
                    }
                }

                _boundingBox = new RectangleF((float)min_x, (float)min_y,
                                      (float)(max_x - min_x),
                                      (float)(max_y - min_y));

                return _boundingBox.Value;
            }
        }

        /// <summary>
        /// Adjusts the model x-y coordinates to within the smallest
        /// rectangle that encompasses all of them.
        /// </summary>
        public void Bound()
        {
            RectangleF bound = BoundingBox;

            // Second pass, adjust the keypoints
            foreach (SurfKeypoint k in _modelKeypoints)
            {
                boundLeft = bound.Left;
                boundTop = bound.Top;

                k.x -= bound.Left;
                k.y -= bound.Top;
            }

            _width = bound.Width;
            _height = bound.Height;

        }

        public void Unbound()
        {
            // Second pass, adjust the keypoints
            foreach (SurfKeypoint k in _modelKeypoints)
            {
                k.x += boundLeft;
                k.y += boundTop;
            }
        }
        
        float boundLeft, boundTop;

        private GeneralMatrix _shapeMatrix = null;

        private GeneralMatrix ShapeMatrix
        {
            get
            {
                if (_shapeMatrix != null) return _shapeMatrix;

                RectangleF _box = this.BoundingBox;
                _shapeMatrix = new GeneralMatrix(3, ObjectOutline.Count);
                PointF[] points = new PointF[ObjectOutline.Count + 1];
                for (int i = 0; i < ObjectOutline.Count; i++)
                {
                    Point pt = ObjectOutline[i];
                    _shapeMatrix.Array[0][i] = pt.X - _box.Left;
                    _shapeMatrix.Array[1][i] = pt.Y - _box.Top;
                    _shapeMatrix.Array[2][i] = 1.0;
                }

                return _shapeMatrix;
            }
        }


        public PointF[] ProjectShape(AffineTransform t)
        {
            return ProjectShape(t, null);
        }

        public PointF[] ProjectShape(AffineTransform t, LiveCamera c)
        {
            if (ObjectOutline == null || ObjectOutline.Count == 0)
                return ProjectSquare(t);
            else
            {
                GeneralMatrix shapeMatrix = ShapeMatrix;
                GeneralMatrix projectedShape = t.H * shapeMatrix;
                projectedShape.HomogeneousNormalizeEquals();
                PointF[] pts = new PointF[projectedShape.ColumnDimension + 1];
                for (int i = 0; i < projectedShape.ColumnDimension; i++)
                {
                    if (c != null)
                    {
                        // Re-distort the object points if a camera is given
                        Point2D reDist = c.Distort(new Point2D(projectedShape.Array[0][i],
                                                               projectedShape.Array[1][i]));
                        pts[i] = reDist.ToPointF();
                    }
                    else
                        pts[i] = new PointF((float)projectedShape.Array[0][i],
                                            (float)projectedShape.Array[1][i]);
                }


                // Connect beginning to end
                pts[projectedShape.ColumnDimension] = pts[0];

                return pts;
            }
        }

        public PointF[] ProjectSquare(WarpModel wM)
        {
            PointF[] points = new PointF[5];
            RectangleF _box = this.BoundingBox;
            _box.Offset(-_box.Left, -_box.Top);

            Point2D corner = new Point2D();

            #region Upper Left
            corner = new Point2D(_box.Left, _box.Top);
            corner = wM.PredictPoint(corner);
            points[0] = corner.ToPointF();
            points[4] = corner.ToPointF();
            #endregion

            #region Upper Right
            corner = new Point2D(_box.Right, _box.Top);
            corner = wM.PredictPoint(corner);
            points[1] = corner.ToPointF();
            #endregion

            #region Lower Right
            corner = new Point2D(_box.Right, _box.Bottom);
            corner = wM.PredictPoint(corner);
            points[2] = corner.ToPointF();
            #endregion

            #region Lower Left
            corner = new Point2D(_box.Left, _box.Bottom);
            corner = wM.PredictPoint(corner);
            points[3] = corner.ToPointF();
            #endregion

            return points;
        }

        public PointF[] ProjectSquare(AffineTransform t)
        {
            PointF[] points = new PointF[5];
            GeneralMatrix corner;
            RectangleF _box = this.BoundingBox;
            _box.Offset(-_box.Left, -_box.Top);

            #region Upper Left
            corner = t.H * Point2D.ToMatrix(_box.Left, _box.Top);
            corner = corner.HomogeneousNormalizeEquals();
            points[0].X = (float)corner.Array[0][0];
            points[0].Y = (float)corner.Array[1][0];
            points[4].X = (float)corner.Array[0][0];
            points[4].Y = (float)corner.Array[1][0];
            #endregion

            #region Upper Right
            corner = t.H * Point2D.ToMatrix(_box.Right, _box.Top);
            corner = corner.HomogeneousNormalizeEquals();
            points[1].X = (float)corner.Array[0][0];
            points[1].Y = (float)corner.Array[1][0];
            #endregion

            #region Lower Right
            corner = t.H * Point2D.ToMatrix(_box.Right, _box.Bottom);
            corner = corner.HomogeneousNormalizeEquals();
            points[2].X = (float)corner.Array[0][0];
            points[2].Y = (float)corner.Array[1][0];
            #endregion

            #region Lower Left
            corner = t.H * Point2D.ToMatrix(_box.Left, _box.Bottom);
            corner = corner.HomogeneousNormalizeEquals();
            points[3].X = (float)corner.Array[0][0];
            points[3].Y = (float)corner.Array[1][0];
            #endregion

            return points;
        }

        protected IList<SurfKeypoint> _modelKeypoints = new SurfKeypoint[0];

        [XmlIgnore]
        public IList<SurfKeypoint> ModelKeypoints
        {
            get { return _modelKeypoints; }
            set { _modelKeypoints = value; }
        }

        public string s_ModelKeypoints
        {
            get { return SerializeKeypoints(_modelKeypoints); }
            set { _modelKeypoints = DeserializeKeypoints(value); }
        }

        #region Efficient SurfKeypoint Serialization
        protected string SerializeKeypoints(IList<SurfKeypoint> keypoints)
        {
            StringBuilder sb = new StringBuilder();
            int m = keypoints.Count;
            for (int i = 0; i < m; i++)
            {
                SurfKeypoint k = keypoints[i];
                sb.AppendFormat("{0:f4} {1:f4} {2:f4} {3:f4} {4} {5:f4}",
                    k.x, k.y, k.scale, k.orientation, k.laplace, k.strength);

                int n = k.i_descriptor.Length;

                for (int j = 0; j < n; j++)
                {
                    sb.Append(" ");
                    sb.Append(k.i_descriptor[j]);
                }

                if (i != (m - 1)) sb.Append("\n");
            }

            return sb.ToString();
        }

        protected SurfKeypoint[] DeserializeKeypoints(string kpString)
        {
            string[] kpLines = kpString.Split(new char[] { '\n' });
            int m = kpLines.Length;
            SurfKeypoint[] kpArr = new SurfKeypoint[m];

            for (int i = 0; i < m; i++)
            {
                string[] kpVals = kpLines[i].Split(new char[] { ' ' });
                int descLen = kpVals.Length - 6;
                if (descLen < 0)
                    throw new Exception("Cannot deserialize one or more keypoints.");

                kpArr[i] = new SurfKeypoint();

                kpArr[i].x = double.Parse(kpVals[0]);
                kpArr[i].y = double.Parse(kpVals[1]);
                kpArr[i].scale = double.Parse(kpVals[2]);
                kpArr[i].orientation = double.Parse(kpVals[3]);
                kpArr[i].laplace = int.Parse(kpVals[4]);
                kpArr[i].strength = double.Parse(kpVals[5]);

                kpArr[i].i_descriptor = new int[descLen];
                int[] i_descriptor = kpArr[i].i_descriptor;

                for (int j = 0; j < descLen; j++)
                {
                    i_descriptor[j] = int.Parse(kpVals[j + 6]);
                }
            }

            return kpArr;
        }
        #endregion





    }

}
